classdef IMUOut2InTest_Inc < matlab.unittest.TestCase
    
    properties (TestParameter)
        NonLinearSFErr = struct('false', false, 'true', true);
        OuterIterStepNum = {4};
        InnerIterStepNum = {4};
    end
    
    methods (Test)
        function vldIMUOut2In_inc(testCase, NonLinearSFErr, OuterIterStepNum, InnerIterStepNum)
            % 验证增量形式的传感器误差计算
            % NOTE: 相比连续形式，由于omegaIBBRate非理想值，在由输出值计算输入值时加速度计杆臂计算误差较大，因此Jf条件数阈值要求比连续形式更低，测试通过判据比连续形式更大
            disp(['NonLinearSFErr = ' int2str(NonLinearSFErr)]);
            
            %% 参数设置
            nSamp = 10000;
            Tl = 0.005; % 5ms
            DupsilonBl = randi([-20, 20], [3 nSamp]) * 9.8 * Tl; % 20g
            DalphaBl = randi([-100, 100], [3 nSamp]) / 180 * pi * Tl; % 100°/s
            DalphaBl1 = DalphaBl - randi([-200, 200], [3 nSamp])/180*pi*Tl^2; % 200°/s^2
            IMUPar = genrandimupar(nSamp, NonLinearSFErr);
            
            %% 计算加入误差的输出值
            [dDupsilonBl, DupsilonTildeSl] = accerror_inc(DupsilonBl, IMUPar.acc, Tl, DalphaBl, DalphaBl1, false);
            DupsilonTildeOutSl_fromErr = NaN(size(DupsilonBl));
            for i=1:length(DupsilonBl)
                DupsilonTildeOutSl_fromErr(:, i) = IMUPar.acc.KScal0(:, i).*((IMUPar.acc.PS0B(:, :, i)')*(dDupsilonBl(:, i)+DupsilonBl(:, i)));
            end
            DupsilonTildeOutSl_direct = IMUPar.acc.KScal0.*DupsilonTildeSl;
            disp('使用误差量计算与直接计算的DupsilonTildeOutSl之相对差绝对值最大值');
            disp(max(abs(relatdiff(DupsilonTildeOutSl_fromErr, DupsilonTildeOutSl_direct)), [], 2)');
            
            [dDalphaBl, DalphaTildeSl] = gyroerror_inc(DalphaBl, IMUPar.gyro, Tl, DupsilonBl, false);
            DalphaTildeOutSl_fromErr = NaN(size(DalphaBl));
            for i=1:length(DalphaBl)
                DalphaTildeOutSl_fromErr(:, i) = IMUPar.gyro.KScal0(:, i).*((IMUPar.gyro.PS0B(:, :, i)')*(dDalphaBl(:, i)+DalphaBl(:, i)));
            end
            DalphaTildeOutSl_direct = IMUPar.gyro.KScal0.*DalphaTildeSl;
            disp('使用误差量计算与直接计算的DalphaTildeOutSl之相对差绝对值最大值');
            disp(max(abs(relatdiff(DalphaTildeOutSl_fromErr, DalphaTildeOutSl_direct)), [], 2)'); % TODO: 陀螺误差为0时该值较大，需要验证
            disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~');
            
            %% 计算输入值
            % 函数验证 线性化法
            innerIterAlgoType = int8(0);
            [DupsilonBl_linear, DalphaBl_linear, accTriadJfCond1, gyroTriadJfCond1] = imuout2in_inc(DupsilonTildeOutSl_direct, DalphaTildeOutSl_direct, IMUPar, Tl, DalphaBl1, ...
                innerIterAlgoType*ones(2, 1, 'int8'), OuterIterStepNum, InnerIterStepNum); %#ok<*NASGU,ASGLU>
            DupsilonBlErr_linear = (DupsilonBl_linear-DupsilonBl)' / 9.8 * 1e6 / Tl;
            disp('线性化法Dupsilon误差绝对值最大值（μg）'); % 应为可以忽略的量级
            disp(max(abs(DupsilonBlErr_linear), [], 1));
            disp('线性化法Dupsilon误差绝对值平均值（μg）'); % 应为可以忽略的量级
            disp(mean(abs(DupsilonBlErr_linear), 1));
            DalphaBlErr_linear = (DalphaBl_linear-DalphaBl)' / pi * 180 * 3600 / Tl;
            disp('线性化法Dalpha误差绝对值最大值（°/h）'); % 应为可以忽略的量级
            disp(max(abs(DalphaBlErr_linear), [], 1));
            disp('线性化法Dalpha误差绝对值平均值（°/h）'); % 应为可以忽略的量级
            disp(mean(abs(DalphaBlErr_linear), 1));
            if ~NonLinearSFErr
                testCase.verifyLessThanOrEqual(max(abs(DupsilonBlErr_linear)), 0.5);
                testCase.verifyLessThanOrEqual(mean(abs(DupsilonBlErr_linear)), 0.001);
                testCase.verifyLessThanOrEqual(max(abs(DalphaBlErr_linear)), 0.002);
                testCase.verifyLessThanOrEqual(mean(abs(DalphaBlErr_linear)), 7e-6);
            end
            disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~');
            
            % 函数验证 牛顿迭代法
            innerIterAlgoType = int8(1);
            [DupsilonBl_newton, DalphaBl_newton, accTriadJfCond2, gyroTriadJfCond2] = imuout2in_inc( DupsilonTildeOutSl_direct, DalphaTildeOutSl_direct, IMUPar, Tl, DalphaBl1, ...
                innerIterAlgoType*ones(2, 1, 'int8'), OuterIterStepNum, InnerIterStepNum ); %#ok<ASGLU>
            DupsilonBlErr_newton = (DupsilonBl_newton-DupsilonBl)' / 9.8 * 1e6 / Tl;
            disp('牛顿迭代法Dupsilon误差绝对值最大值（μg）'); % 应为可以忽略的量级
            disp(max(abs(DupsilonBlErr_newton), [], 1));
            disp('牛顿迭代法Dupsilon误差绝对值平均值（μg）'); % 应为可以忽略的量级
            disp(mean(abs(DupsilonBlErr_newton), 1));
            DalphaBlErr_newton = (DalphaBl_newton-DalphaBl)' / pi * 180 * 3600 / Tl;
            disp('牛顿迭代法Dalpha误差绝对值最大值（°/h）'); % 应为可以忽略的量级
            disp(max(abs(DalphaBlErr_newton), [], 1));
            disp('牛顿迭代法Dalpha误差绝对值平均值（°/h）'); % 应为可以忽略的量级
            disp(mean(abs(DalphaBlErr_newton), 1));
            testCase.verifyLessThanOrEqual(max(abs(DupsilonBlErr_newton)), 0.5);
            testCase.verifyLessThanOrEqual(mean(abs(DupsilonBlErr_newton)), 0.001);
            testCase.verifyLessThanOrEqual(max(abs(DalphaBlErr_newton)), 0.002);
            testCase.verifyLessThanOrEqual(mean(abs(DalphaBlErr_newton)), 7e-6);
            disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~');
        end
    end
end